void nv21_to_yuv444(cv::String nv21_file, cv::Mat & YUV_img, int height, int width)
{
	FILE * fp;
	// fopen_s(&fp, nv21_file.c_str(), "rb");
	fp = fopen(nv21_file.c_str(), "rb");
	// nv21_to_yuv444
	int nv21_size = height * width;
	int half_nv21_size = nv21_size >> 1;
	// int quarter_nv21_size = nv21_size >> 2;
	// int half_height = height >> 1;
	int half_width = width >> 1;
	uchar *Y_block_ptr = (uchar *)malloc(nv21_size * sizeof(uchar));
	uchar *vu_block_ptr = (uchar *)malloc(half_nv21_size * sizeof(uchar));
	// uchar *v_block_ptr = (uchar *)malloc(quarter_nv21_size * sizeof(uchar));
	// uchar *u_block_ptr = (uchar *)malloc(quarter_nv21_size * sizeof(uchar));
	fread(Y_block_ptr, sizeof(uchar), nv21_size, fp);
	fseek(fp, nv21_size, SEEK_SET);
	fread(vu_block_ptr, sizeof(uchar), half_nv21_size, fp);
	// uchar * V_block_ptr = (uchar *)malloc(nv21_size * sizeof(uchar));
	// uchar * U_block_ptr = (uchar *)malloc(nv21_size * sizeof(uchar));

	cv::Vec3b * YUV_img_ptr = (cv::Vec3b *)YUV_img.data;

	for (int i = 0; i < half_nv21_size;)
	{
		for (int j = 0; j < half_width; ++j)
		{
			uchar v_val = *vu_block_ptr++;
			uchar u_val = *vu_block_ptr++;

			// method_3
			cv::Vec3b & yuv_img_pixel_1 = *YUV_img_ptr;
			cv::Vec3b & yuv_img_pixel_3 = *(YUV_img_ptr + width);
			cv::Vec3b & yuv_img_pixel_2 = *(++YUV_img_ptr);
			cv::Vec3b & yuv_img_pixel_4 = *(YUV_img_ptr + width);
			++YUV_img_ptr;

			// Y, U, V
			yuv_img_pixel_1[0] = *Y_block_ptr;
			yuv_img_pixel_3[0] = *(Y_block_ptr + width);
			yuv_img_pixel_2[0] = *(++Y_block_ptr);
			yuv_img_pixel_4[0] = *(Y_block_ptr + width);
			++Y_block_ptr;

			yuv_img_pixel_1[1] = u_val;
			yuv_img_pixel_3[1] = u_val;
			yuv_img_pixel_2[1] = u_val;
			yuv_img_pixel_4[1] = u_val;

			yuv_img_pixel_1[2] = v_val;
			yuv_img_pixel_3[2] = v_val;
			yuv_img_pixel_2[2] = v_val;
			yuv_img_pixel_4[2] = v_val;
			
			i += 2;
		}
		YUV_img_ptr += width;
		Y_block_ptr += width;
	}
}

cv::Mat yuv444_to_nv21(cv::Mat & YUV_srcImg, int height, int width)
{
	int size = height * width;
	int quarter_size = size >> 2;
	int half_height = height >> 1;
	int half_width = width >> 1;
	cv::Mat nv21_img(height + half_height, width, CV_8UC1);
	uchar *y_nv21_img_ptr = nv21_img.data;
	uchar *vu_nv21_img_ptr = nv21_img.data +  size;

	cv::Vec3b * YUV_srcImg_ptr_y = (cv::Vec3b *)YUV_srcImg.data;
	cv::Vec3b * YUV_srcImg_ptr_uv = (cv::Vec3b *)YUV_srcImg.data;


	for (int i = 0; i < half_height; ++i) {
		for (int j = 0; j < half_width; ++j)
		{
			// y
			// Vec3b & YUV_srcImg_pixel_y = *YUV_srcImg_ptr_y++;
			*y_nv21_img_ptr++ = (*YUV_srcImg_ptr_y++)[0];
			*y_nv21_img_ptr++ = (*YUV_srcImg_ptr_y++)[0];
			*y_nv21_img_ptr++ = (*YUV_srcImg_ptr_y++)[0];
			*y_nv21_img_ptr++ = (*YUV_srcImg_ptr_y++)[0];

			// u, v
			// Vec3b & YUV_srcImg_pixel_uv = *YUV_srcImg_ptr_uv++;
			*vu_nv21_img_ptr++ = (*YUV_srcImg_ptr_uv)[2];
			*vu_nv21_img_ptr++ = (*YUV_srcImg_ptr_uv)[1];

			++YUV_srcImg_ptr_uv;
			++YUV_srcImg_ptr_uv;
		}
		YUV_srcImg_ptr_uv += width;
	}
	return nv21_img;
}

int main()
{
	cv::String nv21_file = "";
	int height = 2448;
	int width = 3264;
	
	cv::Mat YUV_srcImg(height, width, CV_8UC3);
	nv21_to_yuv444(nv21_file, YUV_srcImg, height, width);
	
	cv::Mat nv21_img = yuv444_to_nv21(YUV_srcImg, height, width);
	
	return 0;
}
